#include "AStar.h"
#include <Ogre.h>
#include <vector>
#include <deque>

using namespace ITP485;

AStar::AStar(void)
{
}

AStar::~AStar(void)
{
}

std::vector <Node*> AStar::FindPath(Node* start, Node* goal) {
	// make the first "path" with the start node
	//vector <Node*> path ();
	// call Search
}

std::vector <Node*> AStar::Search(std::deque < std::vector <Node*>* >* pathList, Node* goal) {
	// if no paths, return null
	if (pathList->empty()) {
		return;	// fix later
	}

	std::vector < Node* >* path = (*pathList)[0];
	Node* node = (*path)[ path->size() - 1 ];
	if (node == goal) {
		// GOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOAL! AYAYAYAYAAYYAYYYAYA
		return (*path);
		//AYAYAYAYAYAYAYAYAYAYAYAY
	}
	
	// queueing function: append the new neighbors onto the list
	ExpandNode(pathList);

	// QUICK SORT
	QuickSortPathList(pathList);

	// recursive call
	return Search(pathList, goal);
}

void AStar::QueueFunc(std::deque < std::vector <Node*>* >* pathList) {
}

void AStar::ExpandNode (std::deque < std::vector <Node*>* >* pathList) {
	// copy & remove 1st path of pathList
	std::vector < Node* >* path = (*pathList)[0];
	Node* node = (*path)[ path->size() - 1 ];
	pathList->pop_front();

	int numNeighbors = node->neighbors.size();
	for (int i = 0; i < numNeighbors; i++) {
		bool flag = true;

		// if this neighbors isn't a BACKTRACK, make a new path
		// and add it to pathList
		Node* neighbor = node->neighbors[i];
		for(int j = 0; j < path->size(); j++){
			if(neighbor == (*path)[j]){
				flag = false;
				break;
			}			
		}
		if(flag){
			std::vector < Node* >* newPath = new std::vector <Node*>;
			for(int k = 0; k < path->size(); k++){
				newPath->push_back((*path)[k]);
			}
			newPath->push_back(neighbor); 
			pathList->push_back(newPath); 
		}
	}
}

int AStar::GetFVal(std::vector < Node*> * path, Node* goal){
	int g = path->size();
	int h = (goal->position - (*path)[path->size()-1]->position).length();
	int f = g + h;
	return f;
}

void AStar::QuickSortPathList(std::deque < std::vector <Node*>* >* pathList, int left, int right, Node* goal) {
	// WILL IMPLEMENT AND SHIT

	int i = left;
	int j = right;
	std::vector <Node*>* tmp;
	int pivot = GetFVal( (*pathList)[(left + right) / 2], goal);

	// partition
	while (i <= j) {
		while (GetFVal((*pathList)[i]) < pivot)
			i++;
		while (GetFVal((*pathList)[j]) > pivot)
			j--;
		if (i <= j) {
			tmp = (*pathList)[i];
			(*pathList)[i] = (*pathList)[j];
			(*pathList)[j] = tmp;
			i++;
			j--;
		}
	};

	// recursion 
	if (left < j)
		QuickSortPathList(pathList, left, j);
	if (i < right)
		QuickSortPathList(pathList, i, right);

}